; Dit is het programma dat gebruikt is voor de klimmende robot in het labo. Merk op dat 
; door subroutines duidelijke namen te geven de leesbaarheid van programmas sterk verhoogd
; wordt.

	LIST   P=PIC16F877A
	#INCLUDE P16F877A.INC
;======================================

	__CONFIG        _BODEN_ON & _CP_OFF & _PWRTE_ON & _WDT_OFF & _LVP_OFF & _HS_OSC

TIMER1          EQU     H'20'   	
TIMER2          EQU     H'21'   
Counter		EQU	H'22'
StatusRobot	EQU	H'23'
VacuumOn	EQU	H'0000'
Succes		EQU 	H'0001'
Direction	EQU	H'0002'
LastAccident	EQU	H'24'

;Inputs	*******************************
Input			EQU	PORTC	;	0		1
BumperRearRight		EQU 	H'0000' ;	Botsing		No  	
BumperRearLeft		EQU 	H'0001' ;	Botsing		No 
BumperFrontRight	EQU 	H'0002' ;	Botsing		No 
BumperFrontLeft		EQU 	H'0003' ;	Botsing		No 
VacuumSliderFront	EQU 	H'0004'	;	Vacuum OK	Niet OK
VacuumSliderRear	EQU 	H'0005'	;	Vacuum OK	Niet OK
VacuumRotorRight	EQU 	H'0006'	;	Vacuum OK	Niet OK
VacuumRotorLeft		EQU 	H'0007'	;	Vacuum OK	Niet OK

;Outputs ******************************
Output			EQU	PORTB	;	0		1	
ControlLED1		EQU 	H'0000'	;	Uit		Aan
ControlLED2		EQU 	H'0001'	;	Uit		Aan
MoveSlider		EQU 	H'0002'	;	Rotor Voor	Rotor Achter
RotateRotor		EQU 	H'0003'	;	Rotor Rechts	Rotor Links
FeetRotor		EQU 	H'0004'	;	Poten Op	Poten neer
FeetSlider		EQU 	H'0005'	;	Poten Op	Poten neer
SuckerRotor		EQU 	H'0006'	;	
SuckerSlider		EQU 	H'0007'

;Control ******************************
Control			EQU	PORTA
JumperManual		EQU 	H'0000'
JumperVacuum		EQU 	H'0001'
Forward			EQU 	H'0002'
Backward		EQU 	H'0003'
RotLeft			EQU 	H'0004'
RotRight		EQU 	H'0005'


;*
;* Level 1
;*		Main
;* Level 2
;*		StepForward
;*		StepBackward
;*		RotateLeft		
;*		RotateRight	
;*
;* Level 3A
;*		SetSliderForwardFeetRotorUp
;*		SetSliderForwardFeetSliderUp
;*		SetSliderBackwardFeetRotorUp
;*		SetSliderBackwardFeetSliderUp
;* Level 3B
;*		SetRotorLeftFeetRotorUp	
;*		SetRotorLeftFeetSliderUp
;*		SetRotorRightFeetRotorUp
;*		SetRotorRightFeetSliderUp
;*
;* Level 4A
;*		SetSliderForward	
;*		SetSliderBackward
;*		SetRotorLeft	
;*		SetRotorRight	
;* Level 4B
;*		FeetSliderUp
;*		FeetRotorUp
;*		FeetSliderDown
;*		FeetRotorDown
;*
;* Level 5
;* 		Pause
;*

		ORG	0
		GOTO	RESET

; *******************************************************
; *******************************************************
RESET		
		BSF	STATUS,RP0	; reg. Bank 1
		MOVLW	0x00
		MOVWF	TRISB		;RB ports outputs
		MOVWF	PORTB		;RB ports clear
		MOVLW	0x06
		MOVWF	ADCON1		;RA ports dig inputs	
		MOVLW	0x3F
		MOVWF	TRISA		;RA ports inputs
		MOVLW	0xFF
		MOVWF	TRISC		;RC ports inputs
		BCF	STATUS,RP0	; reg. Bank 0
		
		BSF	Output,FeetRotor
		BSF	Output,FeetSlider

		BCF	StatusRobot,VacuumOn
		BTFSC	Control,JumperVacuum
		BSF	StatusRobot,VacuumOn

		CALL 	Delay
		BTFSC	StatusRobot,VacuumOn
		BSF	Output,SuckerRotor
		BTFSC	StatusRobot,VacuumOn
		BSF	Output,SuckerSlider

		BSF	StatusRobot,Direction

MainLoop	
		BCF	StatusRobot,VacuumOn
		BCF	Output,SuckerSlider
		BCF	Output,SuckerRotor	

		BTFSS	Control,JumperVacuum
		GOTO 	NoVacuumWanted
		BSF	StatusRobot,VacuumOn
		BTFSC	Output,FeetSlider
		BSF	Output,SuckerSlider
		BTFSC	Output,FeetRotor
		BSF	Output,SuckerRotor	

NoVacuumWanted
		BTFSS	Control,JumperManual
		GOTO	ManualControl

NextAutoStep
		BTFSS	Control,JumperManual
		GOTO	ManualControl

		BTFSS	StatusRobot,Direction
		CALL	StepBackward
		BTFSC	StatusRobot,Direction
		CALL	StepForward

		BTFSC	StatusRobot,Succes	
		GOTO	NextAutoStep
		BTFSS	LastAccident,BumperRearRight
		GOTO	OtherSide
		BTFSS	LastAccident,BumperFrontLeft
		GOTO	OtherSide

		BTFSS	Control,JumperManual
		GOTO	ManualControl

		CALL	RotateLeft

		BTFSS	StatusRobot,Succes	
		GOTO 	ChangeDirection

		BTFSS	StatusRobot,Direction
		CALL	StepBackward
		BTFSC	StatusRobot,Direction
		CALL	StepForward

		BTFSC	StatusRobot,Succes	
		GOTO	NextAutoStep

		GOTO 	ChangeDirection

OtherSide
		BTFSS	LastAccident,BumperRearLeft
		GOTO	StatusReady
		BTFSS	LastAccident,BumperFrontRight
		GOTO	StatusReady

		BTFSS	Control,JumperManual
		GOTO	ManualControl

		CALL	RotateRight
		BTFSS	StatusRobot,Succes	
		GOTO 	ChangeDirection

		BTFSS	StatusRobot,Direction
		CALL	StepBackward
		BTFSC	StatusRobot,Direction
		CALL	StepForward

		BTFSC	StatusRobot,Succes	
		GOTO	NextAutoStep

		GOTO 	ChangeDirection

ChangeDirection
		BTFSC	StatusRobot,Direction
		GOTO	OtherDirection
		BSF	StatusRobot,Direction	
		GOTO 	StatusReady	
OtherDirection
		BCF	StatusRobot,Direction	
		GOTO 	StatusReady	

ManualControl
		BTFSS	Control,Forward
		CALL	StepForward	
		BTFSS	Control,Backward
		CALL	StepBackward
		BTFSS	Control,RotLeft
		CALL	RotateLeft	
		BTFSS	Control,RotRight
		CALL	RotateRight
StatusReady
		BCF	Output,ControlLED2
		BTFSS	StatusRobot,Succes
		BSF	Output,ControlLED2

		BCF	Output,ControlLED1
		CALL	DelayShort
		CALL	DelayShort
		CALL	DelayShort
		CALL	DelayShort
		CALL	DelayShort
		BSF	Output,ControlLED1


		
		GOTO 	MainLoop


; *******************************************************
; *******************************************************

StepForward	; ***************************************
		BTFSS	Output,MoveSlider
		GOTO	SliderInFrontPosition
		CALL	SetSliderForwardFeetRotorUp
		BTFSS	StatusRobot,Succes
		RETURN
SliderInFrontPosition
		CALL	SetSliderBackwardFeetSliderUp
		RETURN

StepBackward	; ***************************************
		BTFSC	Output,MoveSlider
		GOTO	SliderInBackPosition
		CALL	SetSliderBackwardFeetRotorUp
		BTFSS	StatusRobot,Succes
		RETURN
SliderInBackPosition
		CALL	SetSliderForwardFeetSliderUp
		RETURN

RotateLeft	; ***************************************
		BTFSC	Output,RotateRotor
		GOTO	SliderInRightPosition
		CALL	SetRotorLeftFeetRotorUp
		BTFSS	StatusRobot,Succes
		RETURN
SliderInRightPosition
		CALL	SetRotorRightFeetSliderUp
		RETURN

RotateRight	; ***************************************
		BTFSS	Output,RotateRotor
		GOTO	SliderInLeftPosition
		CALL	SetRotorRightFeetRotorUp
		BTFSS	StatusRobot,Succes
		RETURN
SliderInLeftPosition
		CALL	SetRotorLeftFeetSliderUp
		RETURN





;************************************************************************************
;************************************************************************************
;************************************************************************************
;************************************************************************************



; *******************************************************
; *******************************************************

SetSliderForwardFeetRotorUp ;****************************
		CLRF	LastAccident

		CALL	FeetRotorUp
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound1
		CALL	SetSliderForward
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound1
		CALL	FeetRotorDown
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound1
		BSF	StatusRobot,Succes
		RETURN
ProblemFound1
		CALL	FeetRotorDown
		BCF	StatusRobot,Succes
		RETURN

SetSliderForwardFeetSliderUp ;***************************
		CLRF	LastAccident

		CALL	FeetSliderUp
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound2
		CALL	SetSliderForward
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound2
		CALL	FeetSliderDown
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound2
		BSF	StatusRobot,Succes
		RETURN
ProblemFound2
		CALL	FeetSliderDown
		BCF	StatusRobot,Succes
		RETURN

SetSliderBackwardFeetRotorUp ;***************************
		CLRF	LastAccident

		CALL	FeetRotorUp
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound3
		CALL	SetSliderBackward
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound3
		CALL	FeetRotorDown
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound3
		BSF	StatusRobot,Succes
		RETURN
ProblemFound3
		CALL	FeetRotorDown
		BCF	StatusRobot,Succes
		RETURN

SetSliderBackwardFeetSliderUp ;**************************
		CLRF	LastAccident

		CALL	FeetSliderUp
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound4
		CALL	SetSliderBackward
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound4
		CALL	FeetSliderDown
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound4
		BSF	StatusRobot,Succes
		RETURN
ProblemFound4
		CALL	FeetSliderDown
		BCF	StatusRobot,Succes
		RETURN

SetRotorLeftFeetRotorUp	;********************************
		CLRF	LastAccident

		CALL	FeetRotorUp
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound5
		CALL	SetRotorLeft
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound5
		CALL	FeetRotorDown
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound5
		BSF	StatusRobot,Succes
		RETURN
ProblemFound5
		CALL	FeetRotorDown
		BCF	StatusRobot,Succes
		RETURN

SetRotorLeftFeetSliderUp ;*******************************
		CLRF	LastAccident

		CALL	FeetSliderUp
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound6
		CALL	SetRotorLeft
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound6
		CALL	FeetSliderDown
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound6
		BSF	StatusRobot,Succes
		RETURN
ProblemFound6
		CALL	FeetSliderDown
		BCF	StatusRobot,Succes
		RETURN
	
SetRotorRightFeetRotorUp ;*******************************
		CLRF	LastAccident

		CALL	FeetRotorUp
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound7
		CALL	SetRotorRight
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound7
		CALL	FeetRotorDown
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound7
		BSF	StatusRobot,Succes
		RETURN
ProblemFound7
		CALL	FeetRotorDown
		BCF	StatusRobot,Succes
		RETURN

SetRotorRightFeetSliderUp ;******************************
		CLRF	LastAccident

		CALL	FeetSliderUp
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound8
		CALL	SetRotorRight
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound8
		CALL	FeetSliderDown
		BTFSS	StatusRobot,Succes
		GOTO	ProblemFound8
		BSF	StatusRobot,Succes
		RETURN
ProblemFound8
		CALL	FeetSliderDown
		BCF	StatusRobot,Succes
		RETURN

; *******************************************************
; *******************************************************

SetSliderForward ;***************************************
		BTFSS	Output,MoveSlider
		RETURN
		BCF	Output,MoveSlider
		MOVLW   0xFF 
                MOVWF   TIMER1 
LoopDelay1
		CALL	DelayShort
		BTFSS	Input,BumperRearRight
		GOTO	Accident1
		CALL	DelayShort
		BTFSS	Input,BumperRearLeft
		GOTO	Accident1
		DECFSZ	TIMER1,F
		GOTO	LoopDelay1	
		BSF	StatusRobot,Succes
		RETURN
Accident1
		MOVF	Input,W
		MOVWF	LastAccident

		BSF	Output,MoveSlider	
		CALL	Delay
		BCF	StatusRobot,Succes	
		RETURN

SetSliderBackward ;**************************************
		BTFSC	Output,MoveSlider
		RETURN
		BSF	Output,MoveSlider
		MOVLW   0xFF 
                MOVWF   TIMER1 
LoopDelay2
		CALL	DelayShort
		BTFSS	Input,BumperFrontRight
		GOTO	Accident2
		CALL	DelayShort
		BTFSS	Input,BumperFrontLeft
		GOTO	Accident2
		DECFSZ	TIMER1,F
		GOTO	LoopDelay2	
		BSF	StatusRobot,Succes
		RETURN
Accident2
		MOVF	Input,W
		MOVWF	LastAccident

		BCF	Output,MoveSlider	
		CALL	Delay
		BCF	StatusRobot,Succes	
		RETURN

SetRotorLeft	;****************************************
		BTFSC	Output,RotateRotor
		RETURN
		BSF	Output,RotateRotor
		MOVLW   0xFF 
                MOVWF   TIMER1 
LoopDelay3
		CALL	DelayShort
		BTFSS	Input,BumperFrontRight
		GOTO	Accident3
		CALL	DelayShort
		BTFSS	Input,BumperRearLeft
		GOTO	Accident3
		DECFSZ	TIMER1,F
		GOTO	LoopDelay3	
		BSF	StatusRobot,Succes
		RETURN
Accident3
		MOVF	Input,W
		MOVWF	LastAccident

		BCF	Output,RotateRotor	
		CALL	Delay
		BCF	StatusRobot,Succes	
		RETURN

SetRotorRight	;****************************************
		BTFSS	Output,RotateRotor
		RETURN
		BCF	Output,RotateRotor
		MOVLW   0xFF 
                MOVWF   TIMER1 
LoopDelay4
		CALL	DelayShort
		BTFSS	Input,BumperRearRight
		GOTO	Accident4
		CALL	DelayShort
		BTFSS	Input,BumperFrontLeft
		GOTO	Accident4
		DECFSZ	TIMER1,F
		GOTO	LoopDelay4	
		BSF	StatusRobot,Succes
		RETURN
Accident4
		MOVF	Input,W
		MOVWF	LastAccident

		BSF	Output,RotateRotor	
		CALL	Delay
		BCF	StatusRobot,Succes	
		RETURN

FeetSliderUp	;****************************************
		BTFSS	StatusRobot,VacuumOn
		GOTO	LiftFeet1
		BTFSC	Input,VacuumRotorRight
		GOTO	VacuumOtherNotOK1
		BTFSC	Input,VacuumRotorLeft
		GOTO	VacuumOtherNotOK1
LiftFeet1		
		BCF	Output,SuckerSlider
		BTFSC	StatusRobot,VacuumOn
		CALL	Delay
		BCF	Output,FeetSlider
		Call 	Delay
		BSF	StatusRobot,Succes		
		RETURN
VacuumOtherNotOK1
		BCF	StatusRobot,Succes	
		RETURN

FeetRotorUp	;****************************************
		BTFSS	StatusRobot,VacuumOn
		GOTO	LiftFeet2
		BTFSC	Input,VacuumSliderFront
		GOTO	VacuumOtherNotOK2
		BTFSC	Input,VacuumSliderRear
		GOTO	VacuumOtherNotOK2
LiftFeet2		
		BCF	Output,SuckerRotor
		BTFSC	StatusRobot,VacuumOn
		CALL	Delay
		BCF	Output,FeetRotor
		Call 	Delay
		BSF	StatusRobot,Succes		
		RETURN
VacuumOtherNotOK2
		BCF	StatusRobot,Succes	
		RETURN

FeetSliderDown	;****************************************
		BSF	StatusRobot,Succes	
		BSF	Output,FeetSlider
		BTFSC	StatusRobot,VacuumOn	
		BSF	Output,SuckerSlider
		CALL	Delay
		BTFSS	StatusRobot,VacuumOn		
		RETURN
		MOVLW	D'05'
		MOVWF	Counter
		GOTO	VacuumNotOK1	
CheckVacuum1
		BTFSC	Input,VacuumSliderFront
		GOTO	VacuumNotOK1
		BTFSC	Input,VacuumSliderRear
		GOTO	VacuumNotOK1
		RETURN
VacuumNotOK1
		BCF	Output,FeetRotor		
		CALL	Delay
		BSF	Output,FeetRotor
		CALL	Delay
		DECFSZ	Counter,F
		GOTO	CheckVacuum1
		BCF	StatusRobot,Succes		
		RETURN

FeetRotorDown	;****************************************
		BSF	StatusRobot,Succes	
		BSF	Output,FeetRotor
		BTFSC	StatusRobot,VacuumOn	
		BSF	Output,SuckerRotor
		CALL	Delay
		BTFSS	StatusRobot,VacuumOn		
		RETURN
		MOVLW	D'05'
		MOVWF	Counter
		GOTO	VacuumNotOK2
CheckVacuum2
		BTFSC	Input,VacuumRotorRight
		GOTO	VacuumNotOK2
		BTFSC	Input,VacuumRotorLeft
		GOTO	VacuumNotOK2
		RETURN
VacuumNotOK2
		BCF	Output,FeetSlider		
		CALL	Delay
		BSF	Output,FeetSlider
		CALL	Delay
		DECFSZ	Counter,F
		GOTO	CheckVacuum2
		BCF	StatusRobot,Succes		
		RETURN

; *******************************************************
; *******************************************************


Delay		;****************************************
		MOVLW   D'255'          
                MOVWF   TIMER1          
LoopDelay
		CALL	DelayShort
		DECFSZ	TIMER1,F
		GOTO	LoopDelay	
		RETURN
DelayShort	
		MOVLW	D'255'
		MOVWF	TIMER2
LoopDelayShort
		NOP
		NOP
		NOP
		NOP
		NOP
		NOP
		NOP
		NOP
		NOP  
		DECFSZ  TIMER2,F   
                GOTO    LoopDelayShort            
		RETURN

	        END      	; End of source code.
				; By Ronald Van Ham


 
